/******************************************************************************
 *  Nano-RK, a real-time operating system for sensor networks.
 *  Copyright (C) 2007, Real-Time and Multimedia Lab, Carnegie Mellon University
 *  All rights reserved.
 *
 *  This is the Open Source Version of Nano-RK included as part of a Dual
 *  Licensing Model. If you are unsure which license to use please refer to:
 *  http://www.nanork.org/nano-RK/wiki/Licensing
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, version 2.0 of the License.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 *******************************************************************************/


#include <nrk.h>
#include <include.h>
#include <ulib.h>
#include <stdio.h>
#include <avr/sleep.h>
#include <rt_link.h>
#include <hal.h>
#include <bmac.h>
#include <nrk_defs.h>
#include <nrk_error.h>
#include <ssp_config.h>

#define MAC_ADDR   0
#define MY_CHANNEL 17

nrk_task_type RX_TASK;
NRK_STK rx_task_stack[NRK_APP_STACKSIZE];
void rx_task (void);


nrk_task_type TX_TASK;
NRK_STK tx_task_stack[NRK_APP_STACKSIZE];
void tx_task (void);

void nrk_create_taskset ();

#define MY_RX_SLOT 2
#define MY_TX_SLOT 0
uint8_t rx_buf[RF_MAX_PAYLOAD_SIZE], tx_buf[RF_MAX_PAYLOAD_SIZE];

routingData_pkt rcv;
data_pkt data_rcv;
HGHi_pkt hg_rcv;
HGResponse_pkt hgw_response;
NetworkConfig_pkt nw_cfg;

pktCache_entry pkt_cache_entry[PKT_CACHE_SIZE];

uint8_t node_addr;

//Packet processing functions
void hgw_pkt_process( uint8_t slot );
inline void buildNetConfig_pkt(uint8_t cfg_info);

//cache management functions
void put_in_cache(uint8_t src, uint8_t dest, uint8_t len, uint8_t seq_no);
uint8_t is_in_pkt_cache(uint8_t src, uint8_t dest, uint8_t len, uint8_t seq_no);

int main ()
{
  nrk_setup_ports ();
  nrk_setup_uart (UART_BAUDRATE_115K2);
	
  nrk_init ();
	
  nrk_led_clr (0);
  nrk_led_clr (1);
  nrk_led_clr (2);
  nrk_led_clr (3);
	
  nrk_time_set (0, 0);
	
  rtl_task_config ();							// or RT_link_config whatever protocol you are using
	
  nrk_create_taskset ();
	
  hgw_response.seq_no = 0;
	
  nrk_start ();
	
  return 0;
}

void rx_task ()
{
  uint8_t *local_rx_buf;
  uint8_t length,slot;
  int8_t rssi;
	
  printf ("rx_task PID=%d\r\n", nrk_get_pid ());
	
  rtl_init (RTL_COORDINATOR);
  rtl_set_channel(MY_CHANNEL);
  rtl_set_schedule( RTL_RX, MY_RX_SLOT, 1 );
  rtl_set_schedule( RTL_TX, MY_TX_SLOT, 1 );
  rtl_start();
	
  rtl_rx_pkt_set_buffer(rx_buf, RF_MAX_PAYLOAD_SIZE);
	
  while(!rtl_ready())  nrk_wait_until_next_period(); 
	
  while (1) {
    // Wait until an RX packet is received
    if( rtl_rx_pkt_check()!=0 ){
      nrk_kprintf( PSTR( "Message received\r\n") );
			
      local_rx_buf=rtl_rx_pkt_get(&length, &rssi, &slot);
      printf( "Got Packet on slot %d %d: \r\n",slot,length );
      if ( length == sizeof(routingData_pkt) || length == sizeof(data_pkt) ){
	 nrk_kprintf( PSTR( "start\r\n") );
	 if( length == sizeof(routingData_pkt) ){
	   memcpy(&rcv, &local_rx_buf[PKT_DATA_START], sizeof(routingData_pkt));
	   rtl_rx_pkt_release();
	 }
	 else{
	   memcpy(&data_rcv, &local_rx_buf[PKT_DATA_START], sizeof(data_pkt));
	   rtl_rx_pkt_release();
	 }
	 printf("%d:", rcv.src_addr);
	 if((rcv.is_neighbour & 0x10)){
	   printf("/%d,%d,%d", 0, ( rcv.is_neighbour & 0x01 ), rcv.rssi_value[0]);
	 }
	 if((rcv.is_neighbour & 0x20)){
	   printf("/%d,%d,%d", 1, ( rcv.is_neighbour & 0x02 ), rcv.rssi_value[1]);
	 }
	 if((rcv.is_neighbour & 0x40)){
	   printf("/%d,%d,%d", 2, ( rcv.is_neighbour & 0x04 ), rcv.rssi_value[2]);
	 }
	 if((rcv.is_neighbour & 0x80)){
	   printf("/%d,%d,%d", 3, ( rcv.is_neighbour & 0x08 ), rcv.rssi_value[3]);
	 }
	 printf(":%d, %d, %d", data_rcv.sensor_data[0], data_rcv.sensor_data[1], data_rcv.sensor_data[2]);
	 nrk_kprintf( PSTR( "end\r\n") );
      }

      if ( length == sizeof(HGHi_pkt) ){
	memcpy(&hg_rcv, &local_rx_buf[PKT_DATA_START], sizeof(HGHi_pkt));
	hgw_pkt_process( slot );
	memcpy(&tx_buf[PKT_DATA_START], &hgw_response, sizeof(HGResponse_pkt));
      }
			
      if ( length == sizeof(NetworkConfig_pkt) ){
	memcpy(&nw_cfg, &local_rx_buf[PKT_DATA_START], sizeof(NetworkConfig_pkt));
	if ( is_in_pkt_cache(nw_cfg.src_addr, nw_cfg.dest_addr, sizeof(NetworkConfig_pkt), nw_cfg.seq_no) < 0 ){
	  put_in_cache(nw_cfg.src_addr, nw_cfg.dest_addr, sizeof(NetworkConfig_pkt), nw_cfg.seq_no);
	  node_addr = ( slot/2 );
	  printf("Config Ack received from: %d", node_addr);
	}
      }
      nrk_led_set (ORANGE_LED);
      nrk_led_toggle(BLUE_LED);
      nrk_led_clr (ORANGE_LED);
    }
    else{
      nrk_wait_until_next_period();
    }
  }
}

void tx_task ()
{
  printf ("tx_task PID=%d\r\n", nrk_get_pid ());
	
  while (1) {
    nrk_led_set (GREEN_LED);
    nrk_led_toggle(RED_LED);
		
		
    // send your message
		
    nrk_kprintf( PSTR( "Message sent") );
    nrk_led_clr (GREEN_LED);
    //wait untill next period or next message whether you are using bmac or rt-link
  }
}

void hgw_pkt_process( uint8_t slot )
{
  if(is_in_pkt_cache(hg_rcv.src_addr, hg_rcv.dest_addr, sizeof(HGHi_pkt), hg_rcv.seq_no) < 0){
		
    put_in_cache(hg_rcv.src_addr, hg_rcv.dest_addr, sizeof(HGHi_pkt), hg_rcv.seq_no);
		
    hgw_response.src_addr = MAC_ADDR;
    hgw_response.dest_addr = hg_rcv.src_addr;
    node_addr = ( slot/2 );
    hgw_response.next_hop = node_addr;
    hgw_response.seq_no = hg_rcv.seq_no;
  }
}

inline void buildNetConfig_pkt(uint8_t cfg_info)
{
  nw_cfg.src_addr = MAC_ADDR;
  nw_cfg.dest_addr = BROADCAST;
  nw_cfg.configuration_info = cfg_info;
  nw_cfg.seq_no++;
}

/* inline void init_ssp_pkt(){ */
/*   routing_data.seq_no = 0; */
/*   hi_pkt.seq_no = 0; */
/*   sensor_data.seq_no = 0; */
/*   helloGW_pkt.seq_no = 0; */
/*   ack_pkt.seq_no = 0; */
/* } */

void nrk_create_taskset ()
{
	
	
  RX_TASK.task = rx_task;
  nrk_task_set_stk( &RX_TASK, rx_task_stack, NRK_APP_STACKSIZE);
  RX_TASK.prio = 2;
  RX_TASK.FirstActivation = TRUE;
  RX_TASK.Type = BASIC_TASK;
  RX_TASK.SchType = PREEMPTIVE;
  RX_TASK.period.secs = 1;
  RX_TASK.period.nano_secs = 0;
  RX_TASK.cpu_reserve.secs = 1;
  RX_TASK.cpu_reserve.nano_secs = 500 * NANOS_PER_MS;
  RX_TASK.offset.secs = 0;
  RX_TASK.offset.nano_secs = 0;
  nrk_activate_task (&RX_TASK);
	
	
  /* TX_TASK.task = tx_task; */
  /* nrk_task_set_stk( &TX_TASK, tx_task_stack, NRK_APP_STACKSIZE); */
  /* TX_TASK.prio = 25; */
  /* TX_TASK.FirstActivation = TRUE; */
  /* TX_TASK.Type = BASIC_TASK; */
  /* TX_TASK.SchType = PREEMPTIVE; */
  /* TX_TASK.period.secs = 30;//put appropriate period here */
  /* TX_TASK.period.nano_secs = 0; */
  /* TX_TASK.cpu_reserve.secs = 0; */
  /* TX_TASK.cpu_reserve.nano_secs = 0; */
  /* TX_TASK.offset.secs = 0; */
  /* TX_TASK.offset.nano_secs = 0; */
  /* nrk_activate_task (&TX_TASK); */
}

uint8_t is_in_pkt_cache(uint8_t src, uint8_t dest, uint8_t len, uint8_t seq_no){
  uint8_t i;
  for(i = 0;  i<PKT_CACHE_SIZE; i++){
    if(pkt_cache_entry[i].src_addr == src &&  pkt_cache_entry[i].dest_addr == dest && pkt_cache_entry[i].length == len && pkt_cache_entry[i].seq_no == seq_no)
      return 1;
  }
  return -1;
}

void put_in_cache(uint8_t src, uint8_t dest, uint8_t len, uint8_t seq_no){
  uint8_t i, oldest, age;
  age = oldest = 0;
  for(i = 0;  i<PKT_CACHE_SIZE; i++){
    if(pkt_cache_entry[i].age > age){
      oldest = i;
      age = pkt_cache_entry[i].age;
    }
  }
  pkt_cache_entry[oldest].src_addr = src;
  pkt_cache_entry[oldest].dest_addr = dest;
  pkt_cache_entry[oldest].length = len;
  pkt_cache_entry[oldest].seq_no = seq_no;
	
	
  for(i = 0;  i<PKT_CACHE_SIZE; i++){
    pkt_cache_entry[i].age++;
  }
	
  pkt_cache_entry[oldest].age = 0;
}
